from robomaster import platform, robot
from robomaster import version
import time

import config
import cv2

def sub_battery_info_handler(batter_info, ep_robot):
    percent = batter_info
    # print("Battery: {0}%.".format(percent))
    ep_led = ep_robot.led
    brightness = int(percent * 255 / 100)
    ep_led.set_led(comp="all", r=brightness, g=brightness, b=brightness)

    if not config.show_battery_info_once:
        config.show_battery_info_once = True
        print("===========   Battery Print ================")
        print("")
        print("[localtime] {0}\n Battery = {1}%.".format(
            time.asctime(time.localtime()),
            percent))
        print("")
        print("===========Battery Print Success!===========")

    if (percent < 20):
        print("Battery: {0}%. 请充电！！请充电！！请充电！！".format(percent))

def sub_sensor_info_handler(sub_info):
    d = sub_info
    # print("[+ Sim] tof1:{0}  tof2:{1}  tof3:{2}  tof4:{3}".format(d[0], d[1], d[2], d[3]))

"""
useless: sub_position always reutrn 0 for the Z Component
[link] https://github.com/dji-sdk/RoboMaster-SDK/issues/39

in main()
ep_chassis.sub_position(freq=10, callback=sub_position_handler)
print_position_info()
ep_chassis.unsub_position()
"""
pos = [0,0,0]
def sub_position_handler(position_info):
    global pos
    pos = position_info
    print_position_info()

def print_position_info():
    print("chassis position: x:{0:.3f}, y:{1:.3f}, z:{2:.3f}".format(pos[0], pos[1], pos[2]))

if __name__ == '__main__':
    sdk_version = version.__version__
    print("sdk version:", sdk_version)

    # robomaster.enable_logging_to_file()
    # robomaster.logger.setLevel(robomaster.logging.ERROR)
    
    ep_robot = platform.get_initialized_robot()
    
    ep_led = ep_robot.led
    print("===========connect success!===========")

    time.sleep(0.5)
    ep_version = ep_robot.get_version()
    print("- Robot Version: {0}".format(ep_version))
    time.sleep(0.5)

    # 低电量提示 - 电量<20% 在控制台提示充电
    ep_battery = ep_robot.battery
    ep_battery.sub_battery_info(5, sub_battery_info_handler, ep_robot)
    # time.sleep(0.5)

    # # 测试修改后的 sensor API
    # print("=========== Sensor Test ===========")
    ep_sensor = ep_robot.sensor
    ep_sensor.sub_distance(freq=50, callback=sub_sensor_info_handler)
    
    ep_chassis = ep_robot.chassis
    ep_chassis._dij_move(0,0,360,0,60).wait_for_completed()

    ep_chassis._dij_move(0.5,0,0,0.1,0).wait_for_completed()

    ep_chassis._dij_move(-0.5,0,0,0.1,0).wait_for_completed()
    
    # time.sleep(1)
    # # SIMULATE_MSG.set_online()
    # time.sleep(1)

    
    # print("=========== Sensor Test End ===========")
    
    # 测试修改后的 chassis 即时动作 API
    # print("=========== Chassis Test ===========")
    # ep_chassis = ep_robot.chassis
    # ep_chassis.drive_speed(0.3,0,0,0.5)
    # time.sleep(0.2)
    # ep_chassis.drive_speed(-0.3,0,0,0.5)
    # print("=========== Chassis Test ===========")
    # ep_chassis.drive_speed(0.3,0,0,0.5)
    # time.sleep(0.5)
    # print("=========== Chassis Test ===========")
    # ep_chassis.drive_speed(-0.3,0,0,1)
    # time.sleep(1.5)
    # print("=========== Chassis Test ===========")
    # ep_chassis.move(0.3,0,0,0.3,0).wait_for_completed()
    # ep_chassis.move(-0.3,0,0,0.3,0).wait_for_completed()
    

     # 测试修改后的 chassis 任务控制 API
    # print("=========== Chassis: move() Test ===========")
    # ep_chassis = ep_robot.chassis
    # ep_chassis.move(0,0,180,1,10).wait_for_completed()
    # input("pause")

    ep_chassis.sub_position(freq=50, callback=sub_position_handler)
    time.sleep(1)

    # print("=== exp2 ===")
    # print_position_info()
    # ep_chassis._dij_move(x=-0.5, y=0.0, z=0).wait_for_completed()
    # # ep_chassis._dij_move(x=0.0, y=0.0, z=180).wait_for_completed()
    # print_position_info()
    # for i in range(4):
    #     ep_chassis._dij_move(0.3,0,0,0.5,30).wait_for_completed()
    #     print_position_info()
    #     ep_chassis._dij_move(0,0,90,0.5,30).wait_for_completed()
    #     print_position_info()

    ep_chassis.unsub_position()


    # print_position_info()
    # act1 = ep_chassis._dij_move(0,0,180,0,60)
    # print("a1")
    # print_position_info()
    # act1.wait_for_completed()
    # print("a2")
    # print_position_info()
    # ep_chassis._dij_move(0.2,0,0,0.1,0).wait_for_completed()
    # print("a3")
    # print_position_info()
    # act2 = ep_chassis._dij_move(0,0,-180,0,60)
    # act2.wait_for_completed()
    # print("a4")
    # print_position_info()
    # ep_chassis._dij_move(0.2,0,0,0.1,0).wait_for_completed()
    # print_position_info()

    
    print("=========== Chassis: move() Test ===========")
    # ep_chassis.drive_speed(0.3,0,0,0.5)
    # time.sleep(0.5)
    # print("=========== Chassis Test ===========")
    # ep_chassis.drive_speed(-0.3,0,0,1)
    time.sleep(1.5)
    
    ep_sensor.unsub_distance()
    # print("=========== Chassis Test ===========")
    # ep_chassis.move(0.3,0,0,0.3,0).wait_for_completed()
    # ep_chassis.move(-0.3,0,0,0.3,0).wait_for_completed()



    # 测试视频流数据 
    # ep_camera = ep_robot.camera

    # 每次获取最新的1帧图像显示，并停留1秒
    # ep_chassis.move(x=0, y=0, z=720, z_speed=45)

    # ep_camera.start_video_stream(display=False)
    # for i in range(0, 10):
    #     img = ep_camera.read_cv2_image(strategy="newest")
    #     cv2.imshow("Robot", img)
    #     cv2.imwrite('pic//{0}.jpg'.format(i),img)#写出文件
    #     cv2.waitKey(1)
    #     time.sleep(1)
    # cv2.destroyAllWindows()


    # time.sleep(1)

    # 关闭订阅sub
    # ep_camera.stop_video_stream()

    config.show_battery_info_once = False
    time.sleep(1)
    ep_battery.unsub_battery_info()
    ep_robot.close()
    